/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-27 13:55:58
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-04-22 14:09:59
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\motorset\motorset.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <rtthread.h>
#include "maths.h"
#include "pid.h"
#include "zino.h"
#include "shanwai.h"
#include "statecontrol.h"
#include "fc_error.h"
#include "motorset.h"
#include "usercmd.h"

#define DBG_TAG "motorset"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

uint16_t motoroutput[MotorCount];
const motorMixer_t activeMixer[MotorCount] =
    {
        /**
         * ↑M4↓    ↓M3↑
         *    \    /
         *    /    \
         * ↓M1↑    ↑M2↓
         *
         */
        // {.throttle = 1.0f, .roll = 1.0f, .pitch = 1.0f, .yaw = -1.0f},
        // {.throttle = 1.0f, .roll = -1.0f, .pitch = 1.0f, .yaw = 1.0f},
        // {.throttle = 1.0f, .roll = -1.0f, .pitch = -1.0f, .yaw = -1.0f},
        // {.throttle = 1.0f, .roll = 1.0f, .pitch = -1.0f, .yaw = 1.0f},
        /**
         * ↑M1↓    ↓M4↑
         *    \    /
         *    /    \
         * ↓M2↑    ↑M3↓
         *
         */
        {.throttle = 1.0f, .roll = 1.0f, .pitch = -1.0f, .yaw = 1.0f},
        {.throttle = 1.0f, .roll = 1.0f, .pitch = 1.0f, .yaw = -1.0f},
        {.throttle = 1.0f, .roll = -1.0f, .pitch = 1.0f, .yaw = 1.0f},
        {.throttle = 1.0f, .roll = -1.0f, .pitch = -1.0f, .yaw = -1.0f},

};

void fc_motor_on()
{
    motor_on_all();
}
void fc_motor_off()
{
    motor_off_all();
}

void fc_motor_set_output(fc_control_t *out)
{
    int16_t max = 0, min = 1000;
    out->thrust = constrainf(out->thrust, 0.0f, 900.0f);
    if (FC_ERROR_CHECK(FC_ARMED))
    {
        for (uint8_t i = 0; i < MotorCount; i++)
        {
            motoroutput[i] = (uint16_t)(out->thrust * activeMixer[i].throttle +
                                        out->roll * activeMixer[i].roll +
                                        out->pitch * activeMixer[i].pitch +
                                        out->yaw * activeMixer[i].yaw);
            // motoroutput[i] = (uint16_t)(control->thrust * activeMixer[i].throttle +
            // control->roll * activeMixer[i].roll);

            motoroutput[i] = constrain(motoroutput[i], 0, 1000);
            if (motoroutput[i] > max)
            {
                max = motoroutput[i];
            }
            if (motoroutput[i] < min)
            {
                min = motoroutput[i];
            }
        }
        out->MixerRange = (max - min);
    }
    else if (FC_ERROR_CHECK(FC_ARMING_ALERK) || FC_ERROR_CHECK(FC_OK_TO_ARM))
    {
        for (uint8_t i = 0; i < MotorCount; i++)
        {
            motoroutput[i] = usercmd.cmChannel[CMD_THROTTLE];

            motoroutput[i] = constrain(motoroutput[i], 0, 1000);
        }
    }
    else
    {
        for (uint8_t i = 0; i < MotorCount; i++)
        {
            motoroutput[i] = 0;
        }
    }

    motor_set_duty(1, motoroutput[0]);
    motor_set_duty(2, motoroutput[1]);
    motor_set_duty(3, motoroutput[2]);
    motor_set_duty(4, motoroutput[3]);
}

void fc_motor_task(void *Parameters)
{
    fc_motor_on();
    // motor_set_duty_range(1000);
    while (1)
    {
        fc_motor_set_output(&fc_control);
        motor_set_duty(1, motoroutput[0]);
        motor_set_duty(2, motoroutput[1]);
        motor_set_duty(3, motoroutput[2]);
        motor_set_duty(4, motoroutput[3]);

        // digitalOsc_Head();
        // digitalOsc_printf("f1", (float)motoroutput[0]);
        // digitalOsc_printf("f1", (float)motoroutput[1]);
        // digitalOsc_printf("f1", (float)motoroutput[2]);
        // digitalOsc_printf("f1", (float)motoroutput[3]);
        // digitalOsc_printf("f1", control.pitch);
        // digitalOsc_printf("f1", control.roll);
        // digitalOsc_printf("f1", control.yaw);
        // digitalOsc_printf("f1", control.thrust);
        // digitalOsc_End();

        rt_thread_mdelay(20);
    }
}
void fcMotorTaskEntry()
{
    rt_thread_t fcMotor_thread = rt_thread_create("fcMotor_thread", fc_motor_task, RT_NULL, 1024, 10, 10);
    if (fcMotor_thread != RT_NULL)
    {
        rt_thread_startup(fcMotor_thread);
    }
    else
    {
        LOG_E("fcMotor_thread create failed.");
    }
}

// MSH_CMD_EXPORT(fc_motor_task, open fc motoroutput);
// ZINO_APP_EXPORT(fcMotorTaskEntry);
